package novemberBot.sensor;

/**current filter types: 
 * 						-nofilter: no filter 
 * 						-maxd: any value above (int)maxD = 255
 * 						-median: median of last (int)filterAmount values
 * 						-median2: median of last (int)filterAmount values !=maxD
 * 						-mean: mean of last (int)filterAmount values !=maxD
 * 						-shortest: shortest of last (int)filterAmount values !=maxD
 * 
 * Filtering also includes false negative and <Minrange filtering for every type except nofilter
 * @author Antoine Lennartz*/
public class UsFilter {
	private String FILTER_TYPE="median";//default type
	private int filterAmount=11, maxD;
	private int[] filterData=new int [filterAmount], sortedData=new int [filterAmount];
	private int oldestIndex=0, k=3, count=k, latestData;
	private Object dALock = new Object();
	private boolean newData=false;
	
	/**empty constructor*/
	public UsFilter(){}
	
	/**default filterType used when getData is called without arguments*/
	public void setDefaultFilter(String type){
		FILTER_TYPE=type;
	}
	
	public void setFilterAmount(int fA){
		filterAmount=fA;
	}
	
	public void setMaxD(int maxD){
		this.maxD=maxD;
	}
	
	
	public void newMeasurement(){
		synchronized(dALock){
			for(int i=0; i<filterAmount; i++){
				sortedData[i]=255;
				filterData[i]=255;
			}
		}
	}
	/**new data from the poller
	 * @param distance - new raw data*/
	public void newUnfilteredData(double distance){	
		newUnfilteredData(distance, FILTER_TYPE);
		
	}
	
	/**Adds latest data to the filter
	 * @param distance - sensor data
	 * @param filterType - type to use for data processing*/
	public void newUnfilteredData(double distance, String filterType){	
		latestData=(int)distance;
		newData=true;
		processData(filterType);
	}
	/**fills the relevant arrrays so that response is faser when values are asked for
	 * @param fT - filter Type*/
	private void processData(String fT){
		if(fT.equals("nofilter") || fT.equals("maxd")) return;
		int distance=latestData;
		if(distance>maxD) {
			distance=255;			
			count--;
		}else count=k;
		if(count<=0 || count==k){
			synchronized(dALock){
				filterData[oldestIndex]=distance;
				oldestIndex=(oldestIndex+1)%filterAmount;
			}
		}
		if(fT.equals("median") || fT.equals("shortest") ||  fT.equals("median2")) qSortData();
		
		
	}
	/**gets latest filtered Data, blocks until new data is available
	 * uses default filter type
	 * @return usData*/
	public int getData(){return getData(FILTER_TYPE);}
	/**gets latest filtered Data, blocks until new data is available
	 * @param fT - filterType
	 * @return usData*/
	public int getData(String fT){
		while(newData==false) Thread.yield();
		newData=false;
		int distance=latestData, returnValue=distance;
		if(distance>maxD)
			returnValue=255;	
		if(fT.equals("nofilter")) returnValue=distance;
		if(fT.equals("mean")) returnValue = meanWOMaxD();
		if(fT.equals("median")) returnValue = median();
		if(fT.equals("median2")) returnValue = medianWOMaxD();
		if(fT.equals("shortest")) returnValue = sortedData[0];

		return returnValue;
		
		
		
	}
	
	/**returns mean value of all non zero and non maxD entries in an array
	 * if all entries are 0 returns 255*/
	private int meanWOMaxD(){
		int s=0, c=0;
		synchronized(dALock){
			for(int i=0; i<filterAmount; i++){
				if(filterData[i]!=255){
					s+=filterData[i];
					c++;
				}
			}
			if(c==0)return maxD;
			else return s/c;
		}
		
	}
	/**@return median value of filterData*/
	private int median(){
		synchronized(dALock){
			return sortedData[sortedData.length/2];
		}
	}
	/**@return median value of filterData ignoring all values over cutoff*/
	private int medianWOMaxD(){
		 int k=0, i=0;
		 synchronized(dALock){
			 while(i<filterAmount && sortedData[i]!=255){
				 k++;
				 i++;
			 }
			 return sortedData[(int) (k/2.0)];	 
		 }
	 }
	 
	/**sorting method for data*/
	private void qSortData(){
		synchronized(dALock){
			for(int i=0; i<filterAmount; i++){
				sortedData[i]=filterData[i];
			}
			qSortRBPiv(sortedData, 0, filterAmount-1);
		}
	}
	

	/**quicksort
	 * @param a - array
	 * @param partLB - lowerbound
	 * @param partRB - higherBound*/
	private static void qSortRBPiv(int[] a, int partLB, int partRB){
		int pivIndex=partRB;
		int temp;
		
		if(partLB>=partRB)return;
		
		for(int i=0; i<pivIndex; i++){
			while(a[i]>a[pivIndex]){
				temp=a[pivIndex];
				a[pivIndex]=a[i];
				a[i]=a[pivIndex-1];
				a[pivIndex-1]=temp;
				pivIndex--;
			}
		}
		qSortRBPiv(a, partLB, pivIndex-1);
		qSortRBPiv(a, pivIndex+1, partRB);
	}
}
